#include "NewProtocolCurtisHeLi.h"
#include "Console.h"
#include "MessageTask.h"
#include "GlobalData.h"
#include "stdio.h"

#define FORK_UP_Vel       0x10
#define FORK_DOWN_Vel     0x40 
#define FORK_DOWN_SLOW_VALAVE     0x50      // this value is slow valve 
#define FORK_DOWN_FAST_VALAVE     0x60      // this value is slow&fast valve

int16_t CNewProtocolCurtisHeLi::_speedVal = 0;
int16_t CNewProtocolCurtisHeLi::_forkVel = 0;
int16_t CNewProtocolCurtisHeLi::_lower_flag = 0;
int16_t CNewProtocolCurtisHeLi::_lift_flag = 0;
int8_t CNewProtocolCurtisHeLi::_fork_UpperLimit = 0;
int8_t CNewProtocolCurtisHeLi::_fork_LowerLimit = 0;

CNewProtocolCurtisHeLi::CNewProtocolCurtisHeLi()
{
	_encoderLine = 1024;
}

bool CNewProtocolCurtisHeLi::encode(motorDriverCmdType ct, int32_t val)
{
	CanTxMsg msg;
	msg.StdId = 0x203;
	msg.IDE = CAN_Id_Standard;
	msg.RTR = CAN_RTR_Data;
	msg.DLC = 8;
	
	if (DRV_CMD_TARGET_SPEED == ct) {
		_speedVal = (int16_t)(val * 60.0f / (4.0f * _encoderLine));
		return false;
	} else if(DRV_CMD_TARGET_POS == ct) {
		uint8_t driveRawDir;
		uint16_t driveRawSpeed;
		int16_t steerRawAngle;
		/*******************************************************************************************************************
		 * add control fock motor lift and lower /J3 is 1 /J3、J4 is 1
		 *******************************************************************************************************************/
		if(!(GlobalControl::getChargeState()) && !(GlobalControl::getisCharging())){
			 if(_lift_flag == 1){
				driveRawDir = (_speedVal < 0) ? (0x05 | FORK_UP_Vel): (0x03 | FORK_UP_Vel);  //J4 set1  ??
			}else if(_lower_flag == 1){
				if(_forkVel == 100){
					driveRawDir = (_speedVal < 0) ? (0x05 | FORK_DOWN_FAST_VALAVE): (0x03 | FORK_DOWN_FAST_VALAVE);
				}else if (_forkVel == 50){
					driveRawDir = (_speedVal < 0) ? (0x05 | FORK_DOWN_SLOW_VALAVE): (0x03 | FORK_DOWN_SLOW_VALAVE);  //Heli slow valve
				}else{
					driveRawDir = (_speedVal < 0) ? (0x05 | FORK_DOWN_Vel): (0x03 | FORK_DOWN_Vel);
				}
			}else{
				driveRawDir = (_speedVal < 0) ? 0x05: 0x03;
			}

			driveRawSpeed = (_speedVal < 0) ? -_speedVal : _speedVal;

			steerRawAngle = val;

			if (steerRawAngle > 1200)
				steerRawAngle = 1200;
			else if (steerRawAngle < -1200)
				steerRawAngle = -1200;

			msg.Data[0] = driveRawDir;
			msg.Data[1] = driveRawSpeed & 0xFF;
			msg.Data[2] = driveRawSpeed >> 8;
			msg.Data[3] = 0x0a;			//acc
			msg.Data[4] = 0x0a;			//acc
			msg.Data[5] = (uint8_t)((steerRawAngle) & 0xFF);	//theta L
			msg.Data[6] = (steerRawAngle) >> 8;					//theta H
			msg.Data[7] = GlobalControl::getBatterySoc() * 100;
		}else{
			msg.Data[0] = 0x00;
			msg.Data[1] = 0x00;
			msg.Data[2] = 0x00;
			msg.Data[3] = 0x00;		
			msg.Data[4] = 0x00;		
			msg.Data[5] = 0x00;	
			msg.Data[6] = 0x00;					
			msg.Data[7] = 0x00;
		}
		_canRouter->putMsg(msg);

		msg.StdId = 0x303;
		msg.Data[0] = ((uint8_t)_lift_flag << 1) | ((uint8_t)_lower_flag << 2);
		if (_lift_flag == 1)
		{
			msg.Data[1] = _forkVel & 0xFF;
			msg.Data[2] = _forkVel >> 8;		
		} else {
			msg.Data[1] = 0x00;
			msg.Data[2] = 0x00;
		}

		if (_lower_flag == 1) {
			msg.Data[3] = (uint16_t)(_forkVel * 32.727f) & 0xff;
			msg.Data[4] = (uint16_t)(_forkVel * 32.727f) >> 8;
		} else {
			msg.Data[3] = 0x00;
			msg.Data[4] = 0x00;
		}
		msg.Data[5] = 0x00;							//theta L
		msg.Data[6] = 0x00;							//theta H
		msg.Data[7] = 0x00;

		_canRouter->putMsg(msg);

	} else if (DRV_CMD_FORK_VEL == ct) {
		_forkVel = val;
		if (_forkVel > 0) {
			_lift_flag = true;
			_lower_flag = false;
		} else if (_forkVel < 0) {
			_lift_flag = false;
			_lower_flag = true;
			_forkVel = -_forkVel;
		} else if (_forkVel == 0) {
			_lift_flag = false;
			_lower_flag = false;
		}

		return false;
	}
	return true;
}

int CNewProtocolCurtisHeLi::decode(motorDriverCmdType (&ct)[10], int32_t (&val)[10], const CanRxMsg& canMsg)
{
	int16_t rawSpeed = 0;
	int16_t rawAngle = 0;
	uint8_t wheel_error = 0;
	uint8_t driver_stute = 0;

	memcpy(&rawAngle, &canMsg.Data[2], 2);
	memcpy(&rawSpeed, &canMsg.Data[0], 2);
	memcpy(&wheel_error, &canMsg.Data[4], 1);
	memcpy(&driver_stute, &canMsg.Data[6], 1);

	if (_motorType == 1) {
		ct[0] = DRV_CMD_ACTURAL_SPEED;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;
		ct[4] = DRV_CMD_DRI_EMC;

		val[0] = rawSpeed;
		val[1] = (driver_stute & 0x02) ? 0 : 1;
		val[2] = (wheel_error > 0) ? 1 : 0;
		val[3] = wheel_error;
		val[4] = (driver_stute & 0x01) ? 1 : 0;

		return 5;
	} else if (_motorType == 2) {
		ct[0] = DRV_CMD_ACTURAL_POS;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;
		ct[4] = DRV_CMD_DRI_EMC;

		val[0] = rawAngle;
		val[1] = (driver_stute & 0x02) ? 0 : 1;
		val[2] = (wheel_error > 0) ? 1 : 0;
		val[3] = wheel_error;
		val[4] = (driver_stute & 0x01) ? 1 : 0;

		return 5;
	} else if (_motorType == 6) {
		ct[0] = DRV_CMD_AUTO_MODE;
		ct[1] = DRV_CMD_DRI_EMC;
		
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;
		ct[4] = DRV_CMD_MOTOR_LIMIT;

		val[0] = (driver_stute & 0x02) ? 0 : 1;
		val[1] = (driver_stute & 0x01) ? 1 : 0;
		val[2] = (wheel_error > 0) ? 1 : 0;
		val[3] = wheel_error;
		_fork_UpperLimit = ((driver_stute >> 3 & 0x01) == 0x01) ? 1 : 0;
		_fork_LowerLimit = ((driver_stute >> 4 & 0x01) == 0x01) ? 1 : 0;
		val[4] = (int8_t)_fork_UpperLimit | (((int8_t)_fork_LowerLimit) << 1);
		//Console::Instance()->printf("val[4]_ is  %d\n", val[4]);

		return 5;
	} else {
		return 0;
	}
}

bool CNewProtocolCurtisHeLi::getTargetPosMsg(CanTxMsg* tx_msg, int& num)
{
	return false;
}